
#include "cvf_ImageEnhancement.h"

// [*] C_TRSable_Image

void C_TRSable_Image::CParameters::myInit(
	double xOldCenter, double yOldCenter,
	double xNewCenter, double yNewCenter,
	double xScale, double yScale,
	double RotatingAngle_BEFORE_Rescaling,
	double RotatingAngle_AFTER_Rescaling
	)
{
	this->xOldCenter = xOldCenter; this->yOldCenter = yOldCenter;
	this->xNewCenter = xNewCenter; this->yNewCenter = yNewCenter;
	this->xScale = xScale; this->yScale = yScale;
	this->RotatingAngle_BEFORE_Rescaling = RotatingAngle_BEFORE_Rescaling;
	this->RotatingAngle_AFTER_Rescaling = RotatingAngle_AFTER_Rescaling;

	_cos_ra1 = cos(RotatingAngle_BEFORE_Rescaling);
	_sin_ra1 = sin(RotatingAngle_BEFORE_Rescaling);
	_cos_ra2 = cos(RotatingAngle_AFTER_Rescaling);
	_sin_ra2 = sin(RotatingAngle_AFTER_Rescaling);
	_rv_xs = 1.0 / xScale;
	_rv_ys = 1.0 / yScale;
}

void C_TRSable_Image::CParameters::ComputeSourcePosition(double& DstXonSource, double& DstYonSource, double xNew, double yNew) const
{
	DstXonSource = (xNew - xNewCenter);
	DstYonSource = (yNew - yNewCenter);
	xNew = (DstXonSource * _cos_ra2 - DstYonSource * _sin_ra2) * _rv_xs;
	yNew = (DstYonSource * _cos_ra2 + DstXonSource * _sin_ra2) * _rv_ys;
	DstXonSource = (xNew * _cos_ra1 - yNew * _sin_ra1) + xOldCenter;
	DstYonSource = (yNew * _cos_ra1 + xNew * _sin_ra1) + yOldCenter;
}

void C_TRSable_Image::CParameters::ComputeNewPosition(double& DstNewX, double& DstNewY, double xSrc, double ySrc) const
{
	DstNewX = xSrc - xOldCenter;
	DstNewY = ySrc - yOldCenter;
	xSrc = (DstNewX * _cos_ra1 + DstNewY * _sin_ra1) * xScale;
	ySrc = (DstNewY * _cos_ra1 - DstNewX * _sin_ra1) * yScale;
	DstNewX = (xSrc * _cos_ra2 + ySrc * _sin_ra2) + xNewCenter;
	DstNewY = (ySrc * _cos_ra2 - xSrc * _sin_ra2) + yNewCenter;
}

double C_TRSable_Image::GetPixel(double x, double y, int v, int ExpandingMode)
{
	int ix, iy;
	double fx, fy;
	double ret;

	ix = (int)(floor(x)+(x<0 ? -EPS : +EPS)); fx = x - ix;
	iy = (int)(floor(y)+(y<0 ? -EPS : +EPS)); fy = y - iy;

	if (ExpandingMode == EXPANDING_MODE_TILE)
	{
		ix = (ix%W+W)%W;
		iy = (iy%H+H)%H;
	}
	else if (ExpandingMode == EXPANDING_MODE_EDGE)
	{
		ix = (ix < 0 ? 0 : (ix<W ? ix : W-1));
		iy = (iy < 0 ? 0 : (iy<H ? iy : H-1));
	}
	else if (ExpandingMode == EXPANDING_MODE_COLOR)
	{
		if (ix<0 || ix>=W || iy<0 || iy>=H)
			return DefaultColor.pElement[v];
	}
	else
	{
		//TODO
	}

	x = ix + fx - 0.5;
	y = iy + fy - 0.5;
	ix = (int)(floor(x)+(x<0 ? -EPS : +EPS)); fx = x - ix;
	iy = (int)(floor(y)+(y<0 ? -EPS : +EPS)); fy = y - iy;

	const double *qdaa, *qdab, *qdba, *qdbb;

	qdaa = _imgSrc.data + v * WH;
	if (ix>0) qdaa += ix;
	if (iy>0) qdaa += iy*W;

	qdba = qdaa;
	if (ix>=0 && ix<W-1) qdba += 1;

	qdab = qdaa;
	qdbb = qdba;
	if (iy>=0 && iy<H-1)
	{
		qdab += W;
		qdbb += W;
	}

	ret = CMyMath::DoInterpolation_Bilinear(*qdaa, *qdab, *qdba, *qdbb, fx, fy);

	//const double* qd = imgSrc.data + ix + iy * _W1 + v * _W1H1;
	//ret = CMyMath::DoInterpolation_Bilinear(*qd, *(qd+_W1), *(qd+1), *(qd+_W1+1), fx, fy);

	return ret;
}

// [*] CImagePositionQueryAnswerer::CPixRef

bool CImagePositionQueryAnswerer::CPixRef::CompareBy_X_Y(const CPixRef& a, const CPixRef& b)
{
	if (a.x != b.x) return a.x < b.x;
	return a.y < b.y;
}

// [*] CImagePositionQueryAnswerer

const CImagePositionQueryAnswerer::CPixRef* CImagePositionQueryAnswerer::_MySearch_Y(const CPixRef* Hd, const CPixRef* Tl, int KeyY)
{
	int len = (int)(Tl - Hd);
	if (len < 1) return NULL;
	if (len == 1) return Hd;
	int ih, it, im;
	for ( ih = 0, it = len-1; ih < it; )
	{
		im = (ih + it) >> 1;
		if (Hd[im].y < KeyY)
			ih = im + 1;
		else
			it = im;
	}
	if (it + 1 < len) ++it;
	im = abs(Hd[ih].y - KeyY) - abs(Hd[it].y - KeyY);
	if (im <= 0)
		return Hd + ih;
	else
		return Hd + it;
}

int CImagePositionQueryAnswerer::ComputePixelDistance(int dx, int dy) const
{
	if (DistanceMode == DISTANCE_MODE_EUCLID)
		return dx * dx + dy * dy;
	else
		return abs(dx) + abs(dy);
}

CImagePositionQueryAnswerer::CImagePositionQueryAnswerer()
{
}

size_t CImagePositionQueryAnswerer::GetMemoryUsage() const
{
	return sizeof(int) * 5
		+ sizeof(double) * 2
		+ PixRefList.GetMemoryUsage()
		+ X_PtrList.GetMemoryUsage();
}

void CImagePositionQueryAnswerer::myInit(int W, int H, const CSimpleTypeArray<int>& PixelList, int DistanceMode)
{
	this->DistanceMode = DistanceMode;

	int i, lx;
	PixelAmount = (int)PixelList.Size;
	PixRefList.Resize(PixelAmount);
	CPixRef* ppr;
	const int* qip;
	for ( ppr = PixRefList.pElement, qip = PixelList.pElement, i = 0; i < PixelAmount; ++i, ++qip, ++ppr )
	{
		ppr->index = i;
		ppr->x = *qip % W;
		ppr->y = *qip / W;
	}
	sort(PixRefList.Begin(), PixRefList.End(), CPixRef::CompareBy_X_Y);

	X_Amount = 0;
	X_PtrList.Clear();
	for ( lx = -1, ppr = PixRefList.pElement, i = 0; i < PixelAmount; ++i, ++ppr )
	{
		if (ppr->x <= lx) continue;
		++X_Amount;
		X_PtrList.PushBack(ppr);
		lx = ppr->x;
	}
	X_PtrList.PushBack(ppr);
}

const CImagePositionQueryAnswerer::CPixRef* CImagePositionQueryAnswerer::FindNearestSampledPixel(int SrcPixelXY) const
{
	return FindNearestSampledPixel(SrcPixelXY % W, SrcPixelXY / W);
}

const CImagePositionQueryAnswerer::CPixRef* CImagePositionQueryAnswerer::FindNearestSampledPixel(int SrcX, int SrcY) const
{
	const CPixRef* const* xph = X_PtrList.pElement;

	int ih, it, im;
	for ( ih = 0, it = X_Amount-1; ih < it; )
	{
		im = (ih + it) >> 1;
		if (xph[im]->x < SrcX)
			ih = im + 1;
		else
			it = im;
	}
	im = it;

	const CPixRef* ret;
	const CPixRef* qpr;
	int ans, t;
	ret = _MySearch_Y(xph[im], xph[im+1], SrcY);
	ans = ComputePixelDistance(ret->x - SrcX, ret->y - SrcY);
	ih = im - 1;
	it = im + 1;
	bool ok = true;
	while (ok)
	{
		ok = false;
		if (ih >= 0)
		{
			t = CMyMath::sqr(xph[ih]->x - SrcX);
			if (t >= ans)
				ih = -1;
			else
			{
				ok = true;
				qpr = _MySearch_Y(xph[ih], xph[ih+1], SrcY);
				t = ComputePixelDistance(qpr->x - SrcX, qpr->y - SrcY);
				if (t < ans)
				{
					ans = t;
					ret = qpr;
				}
				--ih;
			}
		}
		if (it < X_Amount)
		{
			t = CMyMath::sqr(xph[it]->x - SrcX);
			if (t >= ans)
				it = X_Amount;
			else
			{
				ok = true;
				qpr = _MySearch_Y(xph[it], xph[it+1], SrcY);
				t = ComputePixelDistance(qpr->x - SrcX, qpr->y - SrcY);
				if (t < ans)
				{
					ans = t;
					ret = qpr;
				}
				++it;
			}
		}
	}

	return ret;
}

//void CScalableImage::BuildScaleConfig(CScaleConfig& dstScaleConfig, double LnScale)
//{
//	BuildScaleConfig(dstScaleConfig, LnScale, LnScale);
//}
//
//void CScalableImage::BuildScaleConfig(CScaleConfig& dstScaleConfig, double LnScaleX, double LnScaleY)
//{
//	dstScaleConfig.LnScaleX = LnScaleX;
//	dstScaleConfig.LnScaleY = LnScaleY;
//	dstScaleConfig.ScaleX = exp(LnScaleX);
//	dstScaleConfig.ScaleY = exp(LnScaleY);
//	dstScaleConfig.fW = W / dstScaleConfig.ScaleX;
//	dstScaleConfig.fH = H / dstScaleConfig.ScaleY;
//	dstScaleConfig.iW = (int)(dstScaleConfig.fW + EPS);
//	dstScaleConfig.iH = (int)(dstScaleConfig.fH + EPS);
//}
//
//void CScalableImage::BuildScaleConfig_i(CScaleConfig& dstScaleConfig, int iWidth, int iHeight)
//{
//	dstScaleConfig.iW = iWidth;
//	dstScaleConfig.iH = iHeight;
//	dstScaleConfig.fW = dstScaleConfig.iW;
//	dstScaleConfig.fH = dstScaleConfig.iH;
//	dstScaleConfig.ScaleX = W / dstScaleConfig.fW;
//	dstScaleConfig.ScaleY = H / dstScaleConfig.fH;
//	dstScaleConfig.LnScaleX = log(dstScaleConfig.ScaleX);
//	dstScaleConfig.LnScaleY = log(dstScaleConfig.ScaleY);
//}
//
//void CScalableImage::BuildOriginalPosition_real(CPositionOnImage& dstPosition, double real_x, double real_y)
//{
//	BuildOriginalPosition_real(dstPosition, real_x, real_y, DefaultScale);
//}
//
//void CScalableImage::BuildOriginalPosition_pixel(CPositionOnImage& dstPosition, int pixel_x, int pixel_y)
//{
//	BuildOriginalPosition_pixel(dstPosition, pixel_x, pixel_y, DefaultScale);
//}
//
//void CScalableImage::BuildOriginalPosition_fraction(CPositionOnImage& dstPosition, double fraction_x, double fraction_y)
//{
//	BuildOriginalPosition_fraction(dstPosition, fraction_x, fraction_y, DefaultScale);
//}
//
//void CScalableImage::BuildOriginalPosition_real(CPositionOnImage& dstPosition, double real_x, double real_y, const CScaleConfig& ScaleConfig)
//{
//	real_x *= ScaleConfig.ScaleX;
//	dstPosition.ix = (int)(real_x);
//	if (dstPosition.ix < 0)
//	{
//		dstPosition.ix = 0;
//		dstPosition.dx = 0.0;
//	}
//	else if (dstPosition.ix < W)
//		dstPosition.dx = real_x - dstPosition.ix;
//	else
//	{
//		dstPosition.ix = W - 1;
//		dstPosition.dx = 1.0;
//	}
//
//	real_y *= ScaleConfig.ScaleY;
//	dstPosition.iy = (int)(real_y);
//	if (dstPosition.iy < 0)
//	{
//		dstPosition.iy = 0;
//		dstPosition.dy = 0.0;
//	}
//	else if (dstPosition.iy < H)
//		dstPosition.dy = real_y - dstPosition.iy;
//	else
//	{
//		dstPosition.iy = H - 1;
//		dstPosition.dy = 1.0;
//	}
//}
//
//void CScalableImage::BuildOriginalPosition_pixel(CPositionOnImage& dstPosition, int pixel_x, int pixel_y, const CScaleConfig& ScaleConfig)
//{
//	BuildOriginalPosition_real(dstPosition, pixel_x + 0.5, pixel_y + 0.5, ScaleConfig);
//}
//
//void CScalableImage::BuildOriginalPosition_fraction(CPositionOnImage& dstPosition, double fraction_x, double fraction_y, const CScaleConfig& ScaleConfig)
//{
//	BuildOriginalPosition_real(dstPosition, fraction_x * ScaleConfig.fW, fraction_y * ScaleConfig.fH, ScaleConfig);
//}
//
//void CScalableImage::BuildNewPosition_real(CPositionOnImage& dstPosition, double real_x, double real_y, const CScaleConfig& ScaleConfig)
//{
//	real_x /= ScaleConfig.ScaleX;
//	dstPosition.ix = (int)(real_x);
//	if (dstPosition.ix < 0)
//	{
//		dstPosition.ix = 0;
//		dstPosition.dx = 0.0;
//	}
//	else if (dstPosition.ix < ScaleConfig.iW)
//		dstPosition.dx = real_x - dstPosition.ix;
//	else
//	{
//		dstPosition.ix = ScaleConfig.iW - 1;
//		dstPosition.dx = 1.0;
//	}
//
//	real_y /= ScaleConfig.ScaleY;
//	dstPosition.iy = (int)(real_y);
//	if (dstPosition.iy < 0)
//	{
//		dstPosition.iy = 0;
//		dstPosition.dy = 0.0;
//	}
//	else if (dstPosition.iy < ScaleConfig.iH)
//		dstPosition.dy = real_y - dstPosition.iy;
//	else
//	{
//		dstPosition.iy = ScaleConfig.iH - 1;
//		dstPosition.dy = 1.0;
//	}
//}
//
//void CScalableImage::BuildNewPosition_pixel(CPositionOnImage& dstPosition, int pixel_x, int pixel_y, const CScaleConfig& ScaleConfig)
//{
//	BuildNewPosition_real(dstPosition, pixel_x + 0.5, pixel_y + 0.5, ScaleConfig);
//}
//
//void CScalableImage::BuildNewPosition_fraction(CPositionOnImage& dstPosition, double fraction_x, double fraction_y, const CScaleConfig& ScaleConfig)
//{
//	BuildNewPosition_real(dstPosition, fraction_x / ScaleConfig.fW, fraction_y / ScaleConfig.fH, ScaleConfig);
//}
//
//double CScalableImage::GetPixelValue(const CPositionOnImage& OriginalPosition, int v)
//{
//	const double* pd = imgSrc.data + OriginalPosition.ix + OriginalPosition.iy * W1 + v * W1H1;
//	return CMyMath::DoInterpolation_Bilinear(*pd, *(pd+W1), *(pd+1), *(pd+W1+1), OriginalPosition.dx, OriginalPosition.dy);
//}

